
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_baro_ms5611.c
  * @author     baiyang
  * @date       2021-11-21
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_baro_ms5611.h"

#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
/* write to one of these addresses to start pressure conversion */
#define ADDR_CMD_CONVERT_D1_OSR256  0x40
#define ADDR_CMD_CONVERT_D1_OSR512  0x42
#define ADDR_CMD_CONVERT_D1_OSR1024 0x44
#define ADDR_CMD_CONVERT_D1_OSR2048 0x46
#define ADDR_CMD_CONVERT_D1_OSR4096 0x48

/* write to one of these addresses to start temperature conversion */
#define ADDR_CMD_CONVERT_D2_OSR256  0x50
#define ADDR_CMD_CONVERT_D2_OSR512  0x52
#define ADDR_CMD_CONVERT_D2_OSR1024 0x54
#define ADDR_CMD_CONVERT_D2_OSR2048 0x56
#define ADDR_CMD_CONVERT_D2_OSR4096 0x58

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool _init(sensor_baro_ms56xx *baro);
static void _update(sensor_baro_backend *baro_backend);
static void _timer(void *parameter);
static uint32_t _read_adc(sensor_baro_ms56xx *ms56xx);
static void _update_and_wrap_accumulator(uint32_t *accum, uint32_t val, uint8_t *count, uint8_t max_count);
static void _calculate_5611(sensor_baro_ms56xx *ms56xx);
static void _calculate_5607(sensor_baro_ms56xx *ms56xx);
static void _calculate_5637(sensor_baro_ms56xx *ms56xx);
static void _calculate_5837(sensor_baro_ms56xx *ms56xx);
static uint16_t _read_prom_word(sensor_baro_ms56xx *ms56xx, uint8_t word);
static bool _read_prom_5611(sensor_baro_ms56xx *ms56xx, uint16_t prom[8]);
static bool _read_prom_5637(sensor_baro_ms56xx *ms56xx, uint16_t prom[8]);
/*----------------------------------variable----------------------------------*/
static struct sensor_baro_backend_ops ms56xx_ops;

static const uint8_t CMD_MS56XX_RESET = 0x1E;
static const uint8_t CMD_MS56XX_READ_ADC = 0x00;

/* PROM start address */
static const uint8_t CMD_MS56XX_PROM = 0xA0;

/*
  use an OSR of 1024 to reduce the self-heating effect of the
  sensor. Information from MS tells us that some individual sensors
  are quite sensitive to this effect and that reducing the OSR can
  make a big difference
 */
static const uint8_t ADDR_CMD_CONVERT_PRESSURE = ADDR_CMD_CONVERT_D1_OSR1024;
static const uint8_t ADDR_CMD_CONVERT_TEMPERATURE = ADDR_CMD_CONVERT_D2_OSR1024;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_baro_ms56xx_ctor(sensor_baro_ms56xx *ms56xx, gp_device_t dev,  enum MS56XX_TYPE ms56xx_type)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(ms56xx, 0, sizeof(sensor_baro_ms56xx));

    sensor_baro_backend_ctor(&ms56xx->backend, "ms56xx");

    ms56xx_ops = baro_backend_ops;

    ms56xx_ops.update = _update;

    ms56xx->backend.ops = &ms56xx_ops;

    ms56xx->_dev = dev;
    ms56xx->_ms56xx_type = ms56xx_type;
}

void sensor_baro_ms56xx_destructor(sensor_baro_ms56xx *ms56xx)
{
    sensor_baro_backend_destructor(&ms56xx->backend);
}

sensor_baro_backend *sensor_baro_ms56xx_probe(gp_device_t dev, enum MS56XX_TYPE ms56xx_type)
{
    if (!dev) {
        return NULL;
    }
    sensor_baro_ms56xx *sensor = (sensor_baro_ms56xx *)rt_malloc(sizeof(sensor_baro_ms56xx));
    sensor_baro_ms56xx_ctor(sensor, dev, ms56xx_type);
    
    if (!sensor || !_init(sensor)) {
        sensor_baro_ms56xx_destructor(sensor);
        rt_free(sensor);
        return NULL;
    }
    return (sensor_baro_backend *)sensor;
}

static bool _init(sensor_baro_ms56xx *ms56xx)
{
    if (!ms56xx->_dev) {
        return false;
    }

    //_dev->get_semaphore()->take_blocking();

    // high retries for init
    //_dev->set_retries(10);
    
    uint16_t prom[8];
    bool prom_read_ok = false;

    devmgr_transfer(ms56xx->_dev, &CMD_MS56XX_RESET, 1, NULL, 0);
    rt_thread_mdelay(4);
    
    const char *name = "MS5611";
    switch (ms56xx->_ms56xx_type) {
    case BARO_MS5607:
        name = "MS5607";
    case BARO_MS5611:
        prom_read_ok = _read_prom_5611(ms56xx, prom);
        break;
    case BARO_MS5837:
        name = "MS5837";
        prom_read_ok = _read_prom_5637(ms56xx, prom);
        break;
    case BARO_MS5637:
        name = "MS5637";
        prom_read_ok = _read_prom_5637(ms56xx, prom);
        break;
    }

    if (!prom_read_ok) {
        //_dev->get_semaphore()->give();
        return false;
    }

    console_printf("%s found on bus %u address 0x%02x\n", name, ms56xx->_dev->d.devid_s.bus, ms56xx->_dev->d.devid_s.address);

    // Save factory calibration coefficients
    ms56xx->_cal_reg.c1 = prom[1];
    ms56xx->_cal_reg.c2 = prom[2];
    ms56xx->_cal_reg.c3 = prom[3];
    ms56xx->_cal_reg.c4 = prom[4];
    ms56xx->_cal_reg.c5 = prom[5];
    ms56xx->_cal_reg.c6 = prom[6];

    // Send a command to read temperature first
    devmgr_transfer(ms56xx->_dev, &ADDR_CMD_CONVERT_TEMPERATURE, 1, NULL, 0);
    ms56xx->_state = 0;

    memset(&ms56xx->_accum, 0, sizeof(ms56xx->_accum));

    ms56xx->_instance = sensor_baro_register_sensor();

    devmgr_set_device_type(ms56xx->_dev, DEVTYPE_BARO_MS5611);
    sensor_baro_backend_set_bus_id(ms56xx->_instance, devmgr_get_bus_id(ms56xx->_dev));

    if (ms56xx->_ms56xx_type == BARO_MS5837) {
        sensor_baro_set_type(ms56xx->_instance, BARO_TYPE_WATER);
    }

    // lower retries for run
    //_dev->set_retries(3);
    
    //_dev->get_semaphore()->give();

    /* Request 100Hz update */
    devmgr_register_periodic_callback(ms56xx->_dev, 10 * AP_USEC_PER_MSEC, _timer, ms56xx);
    return true;
}

static void _update(sensor_baro_backend *baro_backend)
{
    sensor_baro_ms56xx *ms56xx = (sensor_baro_ms56xx *)baro_backend;

    uint32_t sD1, sD2;
    uint8_t d1count, d2count;

    {
        sensor_baro_backend_mutex_take(&ms56xx->backend);
        
        if (ms56xx->_accum.d1_count == 0) {
            sensor_baro_backend_mutex_release(&ms56xx->backend);
            return;
        }

        sD1 = ms56xx->_accum.s_D1;
        sD2 = ms56xx->_accum.s_D2;
        d1count = ms56xx->_accum.d1_count;
        d2count = ms56xx->_accum.d2_count;
        memset(&ms56xx->_accum, 0, sizeof(ms56xx->_accum));

        sensor_baro_backend_mutex_release(&ms56xx->backend);
    }

    if (d1count != 0) {
        ms56xx->_D1 = ((float)sD1) / d1count;
    }
    if (d2count != 0) {
        ms56xx->_D2 = ((float)sD2) / d2count;
    }

    switch (ms56xx->_ms56xx_type) {
    case BARO_MS5607:
        _calculate_5607(ms56xx);
        break;
    case BARO_MS5611:
        _calculate_5611(ms56xx);
        break;
    case BARO_MS5637:
        _calculate_5637(ms56xx);
        break;
    case BARO_MS5837:
        _calculate_5837(ms56xx);
    }
}

/*
 * Read the sensor with a state machine
 * We read one time temperature (state=0) and then 4 times pressure (states 1-4)
 *
 * Temperature is used to calculate the compensated pressure and doesn't vary
 * as fast as pressure. Hence we reuse the same temperature for 4 samples of
 * pressure.
*/
static void _timer(void *parameter)
{
    sensor_baro_ms56xx *ms56xx = (sensor_baro_ms56xx *)parameter;

    uint8_t next_cmd;
    uint8_t next_state;
    uint32_t adc_val = _read_adc(ms56xx);

    /*
     * If read fails, re-initiate a read command for current state or we are
     * stuck
     */
    if (adc_val == 0) {
        next_state = ms56xx->_state;
    } else {
        next_state = (ms56xx->_state + 1) % 5;
    }

    next_cmd = next_state == 0 ? ADDR_CMD_CONVERT_TEMPERATURE
                               : ADDR_CMD_CONVERT_PRESSURE;
    if (!devmgr_transfer(ms56xx->_dev, &next_cmd, 1, NULL, 0)) {
        return;
    }

    /* if we had a failed read we are all done */
    if (adc_val == 0 || adc_val == 0xFFFFFF) {
        // a failed read can mean the next returned value will be
        // corrupt, we must discard it. This copes with MISO being
        // pulled either high or low
        ms56xx->_discard_next = true;
        return;
    }

    if (ms56xx->_discard_next) {
        ms56xx->_discard_next = false;
        ms56xx->_state = next_state;
        return;
    }

    sensor_baro_backend_mutex_take(&ms56xx->backend);

    if (ms56xx->_state == 0) {
        _update_and_wrap_accumulator(&ms56xx->_accum.s_D2, adc_val,
                                     &ms56xx->_accum.d2_count, 32);
    } else if (sensor_baro_backend_pressure_ok(&ms56xx->backend, adc_val)) {
        _update_and_wrap_accumulator(&ms56xx->_accum.s_D1, adc_val,
                                     &ms56xx->_accum.d1_count, 128);
    }
    
    ms56xx->_state = next_state;

    sensor_baro_backend_mutex_release(&ms56xx->backend);
}

static uint32_t _read_adc(sensor_baro_ms56xx *ms56xx)
{
    uint8_t val[3];
    if (!devmgr_transfer(ms56xx->_dev, &CMD_MS56XX_READ_ADC, 1, val, sizeof(val))) {
        return 0;
    }
    return (val[0] << 16) | (val[1] << 8) | val[2];
}

static void _update_and_wrap_accumulator(uint32_t *accum, uint32_t val, uint8_t *count, uint8_t max_count)
{
    *accum += val;
    *count += 1;
    if (*count == max_count) {
        *count = max_count / 2;
        *accum = *accum / 2;
    }
}

// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
static void _calculate_5611(sensor_baro_ms56xx *ms56xx)
{
    float dT;
    float TEMP;
    float OFF;
    float SENS;

    // we do the calculations using floating point allows us to take advantage
    // of the averaging of D1 and D1 over multiple samples, giving us more
    // precision
    dT = ms56xx->_D2-(((uint32_t)ms56xx->_cal_reg.c5)<<8);
    TEMP = (dT * ms56xx->_cal_reg.c6)/8388608;
    OFF = ms56xx->_cal_reg.c2 * 65536.0f + (ms56xx->_cal_reg.c4 * dT) / 128;
    SENS = ms56xx->_cal_reg.c1 * 32768.0f + (ms56xx->_cal_reg.c3 * dT) / 256;

    TEMP += 2000;

    if (TEMP < 2000) {
        // second order temperature compensation when under 20 degrees C
        float T2 = (dT*dT) / 0x80000000;
        float Aux = sq(TEMP-2000.0);
        float OFF2 = 2.5f*Aux;
        float SENS2 = 1.25f*Aux;
        if (TEMP < -1500) {
            // extra compensation for temperatures below -15C
            OFF2 += 7 * sq(TEMP+1500);
            SENS2 += sq(TEMP+1500) * 11.0*0.5;
        }
        TEMP = TEMP - T2;
        OFF = OFF - OFF2;
        SENS = SENS - SENS2;
    }


    float pressure = (ms56xx->_D1*SENS/2097152 - OFF)/32768;
    float temperature = TEMP * 0.01f;
    sensor_baro_backend_copy_to_frontend(ms56xx->_instance, pressure, temperature);
}

// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
static void _calculate_5607(sensor_baro_ms56xx *ms56xx)
{
    float dT;
    float TEMP;
    float OFF;
    float SENS;

    // we do the calculations using floating point allows us to take advantage
    // of the averaging of D1 and D1 over multiple samples, giving us more
    // precision
    dT = ms56xx->_D2-(((uint32_t)ms56xx->_cal_reg.c5)<<8);
    TEMP = (dT * ms56xx->_cal_reg.c6)/8388608;
    OFF = ms56xx->_cal_reg.c2 * 131072.0f + (ms56xx->_cal_reg.c4 * dT) / 64;
    SENS = ms56xx->_cal_reg.c1 * 65536.0f + (ms56xx->_cal_reg.c3 * dT) / 128;

    TEMP += 2000;

    if (TEMP < 2000) {
        // second order temperature compensation when under 20 degrees C
        float T2 = (dT*dT) / 0x80000000;
        float Aux = sq(TEMP-2000);
        float OFF2 = 61.0f*Aux/16.0f;
        float SENS2 = 2.0f*Aux;
        if (TEMP < -1500) {
            OFF2 += 15 * sq(TEMP+1500);
            SENS2 += 8 * sq(TEMP+1500);
        }
        TEMP = TEMP - T2;
        OFF = OFF - OFF2;
        SENS = SENS - SENS2;
    }

    float pressure = (ms56xx->_D1*SENS/2097152 - OFF)/32768;
    float temperature = TEMP * 0.01f;
    sensor_baro_backend_copy_to_frontend(ms56xx->_instance, pressure, temperature);
}

// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
static void _calculate_5637(sensor_baro_ms56xx *ms56xx)
{
    int32_t dT, TEMP;
    int64_t OFF, SENS;
    int32_t raw_pressure = ms56xx->_D1;
    int32_t raw_temperature = ms56xx->_D2;

    dT = raw_temperature - (((uint32_t)ms56xx->_cal_reg.c5) << 8);
    TEMP = 2000 + ((int64_t)dT * (int64_t)ms56xx->_cal_reg.c6) / 8388608;
    OFF = (int64_t)ms56xx->_cal_reg.c2 * (int64_t)131072 + ((int64_t)ms56xx->_cal_reg.c4 * (int64_t)dT) / (int64_t)64;
    SENS = (int64_t)ms56xx->_cal_reg.c1 * (int64_t)65536 + ((int64_t)ms56xx->_cal_reg.c3 * (int64_t)dT) / (int64_t)128;

    if (TEMP < 2000) {
        // second order temperature compensation when under 20 degrees C
        int32_t T2 = ((int64_t)3 * ((int64_t)dT * (int64_t)dT) / (int64_t)8589934592);
        int64_t aux = (TEMP - 2000) * (TEMP - 2000);
        int64_t OFF2 = 61 * aux / 16;
        int64_t SENS2 = 29 * aux / 16;

        if (TEMP < -1500) {
            OFF2 += 17 * sq(TEMP+1500);
            SENS2 += 9 * sq(TEMP+1500);
        }
        
        TEMP = TEMP - T2;
        OFF = OFF - OFF2;
        SENS = SENS - SENS2;
    }

    int32_t pressure = ((int64_t)raw_pressure * SENS / (int64_t)2097152 - OFF) / (int64_t)32768;
    float temperature = TEMP * 0.01f;
    sensor_baro_backend_copy_to_frontend(ms56xx->_instance, (float)pressure, temperature);
}

// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
static void _calculate_5837(sensor_baro_ms56xx *ms56xx)
{
    int32_t dT, TEMP;
    int64_t OFF, SENS;
    int32_t raw_pressure = ms56xx->_D1;
    int32_t raw_temperature = ms56xx->_D2;

    // note that MS5837 has no compensation for temperatures below -15C in the datasheet

    dT = raw_temperature - (((uint32_t)ms56xx->_cal_reg.c5) << 8);
    TEMP = 2000 + ((int64_t)dT * (int64_t)ms56xx->_cal_reg.c6) / 8388608;
    OFF = (int64_t)ms56xx->_cal_reg.c2 * (int64_t)65536 + ((int64_t)ms56xx->_cal_reg.c4 * (int64_t)dT) / (int64_t)128;
    SENS = (int64_t)ms56xx->_cal_reg.c1 * (int64_t)32768 + ((int64_t)ms56xx->_cal_reg.c3 * (int64_t)dT) / (int64_t)256;

    if (TEMP < 2000) {
        // second order temperature compensation when under 20 degrees C
        int32_t T2 = ((int64_t)3 * ((int64_t)dT * (int64_t)dT) / (int64_t)8589934592);
        int64_t aux = (TEMP - 2000) * (TEMP - 2000);
        int64_t OFF2 = 3 * aux / 2;
        int64_t SENS2 = 5 * aux / 8;

        TEMP = TEMP - T2;
        OFF = OFF - OFF2;
        SENS = SENS - SENS2;
    }

    int32_t pressure = ((int64_t)raw_pressure * SENS / (int64_t)2097152 - OFF) / (int64_t)8192;
    pressure = pressure * 10; // MS5837 only reports to 0.1 mbar
    float temperature = TEMP * 0.01f;

    sensor_baro_backend_copy_to_frontend(ms56xx->_instance, (float)pressure, temperature);
}

static uint16_t _read_prom_word(sensor_baro_ms56xx *ms56xx, uint8_t word)
{
    const uint8_t reg = CMD_MS56XX_PROM + (word << 1);
    uint8_t val[2];
    if (!devmgr_transfer(ms56xx->_dev, &reg, 1, val, sizeof(val))) {
        return 0;
    }
    return (val[0] << 8) | val[1];
}

static bool _read_prom_5611(sensor_baro_ms56xx *ms56xx, uint16_t prom[8])
{
    /*
     * MS5611-01BA datasheet, CYCLIC REDUNDANCY CHECK (CRC): "MS5611-01BA
     * contains a PROM memory with 128-Bit. A 4-bit CRC has been implemented
     * to check the data validity in memory."
     *
     * CRC field must me removed for CRC-4 calculation.
     */
    bool all_zero = true;
    for (uint8_t i = 0; i < 8; i++) {
        prom[i] = _read_prom_word(ms56xx, i);
        if (prom[i] != 0) {
            all_zero = false;
        }
    }

    if (all_zero) {
        return false;
    }

    /* save the read crc */
    const uint16_t crc_read = prom[7] & 0xf;

    /* remove CRC byte */
    prom[7] &= 0xff00;

    return crc_read == math_crc_crc4(prom);
}

static bool _read_prom_5637(sensor_baro_ms56xx *ms56xx, uint16_t prom[8])
{
    /*
     * MS5637-02BA03 datasheet, CYCLIC REDUNDANCY CHECK (CRC): "MS5637
     * contains a PROM memory with 112-Bit. A 4-bit CRC has been implemented
     * to check the data validity in memory."
     *
     * 8th PROM word must be zeroed and CRC field removed for CRC-4
     * calculation.
     */
    bool all_zero = true;
    for (uint8_t i = 0; i < 7; i++) {
        prom[i] = _read_prom_word(ms56xx, i);
        if (prom[i] != 0) {
            all_zero = false;
        }
    }

    if (all_zero) {
        return false;
    }

    prom[7] = 0;

    /* save the read crc */
    const uint16_t crc_read = (prom[0] & 0xf000) >> 12;

    /* remove CRC byte */
    prom[0] &= ~0xf000;

    return crc_read == math_crc_crc4(prom);
}

/*------------------------------------test------------------------------------*/


